/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Common.h"
#include <cmath>

namespace Robotic { namespace Math{
	//Predefine values
	ROBOTIC_MATH_API const double Gravity = 9.81;
	ROBOTIC_MATH_API const double PI = 3.1415926535897932384626433832795;
	ROBOTIC_MATH_API const double DegToRad = 0.01745329251994329576923690768488;
	ROBOTIC_MATH_API const double RadtoDeg = 57.2957795130823208767981548141052;
	ROBOTIC_MATH_API const double Epsilon = 0.000001;

	//Basic
	//ROBOTIC_MATH_API double sin(double a)
	//{
	//	return ::sin(a);
	//}
	//ROBOTIC_MATH_API double cos(double a) 
	//{
	//	return ::cos(a);
	//}
	//ROBOTIC_MATH_API double tan(double a) 
	//{
	//	return ::tan(a);
	//}
	//ROBOTIC_MATH_API double asin(double a)
	//{
	//	return ::asin(a);
	//}
	//ROBOTIC_MATH_API double acos(double a)
	//{
	//	return ::acos(a);
	//}
	//ROBOTIC_MATH_API double atan(double a)
	//{
	//	return ::atan(a);
	//}
	//ROBOTIC_MATH_API double atan2(double a,double b) 
	//{
	//	return ::atan2(a,b);
	//}
	//ROBOTIC_MATH_API double sinh(double a) 
	//{
	//	return ::sinh(a);
	//}
	//ROBOTIC_MATH_API double cosh(double a)
	//{
	//	return ::cosh(a);
	//}
	//ROBOTIC_MATH_API double tanh(double a) 
	//{
	//	return ::tanh(a);
	//}
	//ROBOTIC_MATH_API double fabs(double a)
	//{
	//	return ::fabs(a);
	//}

	ROBOTIC_MATH_API double sign(double arg)
	{
		return (arg<0)?(-1):(1);
	}

	//ROBOTIC_MATH_API double exp(double a) 
	//{
	//	return ::exp(a);
	//}
	//ROBOTIC_MATH_API double log(double a) 
	//{
	//	return ::log(a);
	//}
	//ROBOTIC_MATH_API double pow(double a,double b) 
	//{
	//	return ::pow(a,b);
	//}

	ROBOTIC_MATH_API double sqr(double a) 
	{
		return a * a;
	}

	//ROBOTIC_MATH_API double sqrt(double a)
	//{
	//	return ::sqrt(a);
	//}


	//Almost Equal
	ROBOTIC_MATH_API bool Equal(const double a,const double b,const double eps)
	{
		double tmp=(a-b);
		return ((eps>tmp)&& (tmp>-eps) );
	}	

	//Add Delta
	ROBOTIC_MATH_API double AddDelta(const double a,const double da,const double dt)
	{
		return a+da*dt;
	}

	//Diff
	ROBOTIC_MATH_API double Diff(const double a,const double b,const double dt)
	{
		return (b-a)/dt;
	}

	//Random
	ROBOTIC_MATH_API void Random(double& a)
	{
		a = 1.98*rand()/(double)RAND_MAX -0.99;
	}
	//PosRandom
	ROBOTIC_MATH_API void PosRandom(double& a)
	{
		a = 0.001+0.99*rand()/(double)RAND_MAX;
	}
}}